#include "Interpolation.h"

#include "DebugHelper.h"

using namespace Eigen;

Eigen::VectorXd DIM_NAMESPACE(One)::Restriction(Operators::Restriction ope, const Eigen::VectorXd &v)
{ // n+1
    int n = v.size() - 1;
    VectorXd res = VectorXd::Zero(n / 2 + 1);
    if (ope == 1)
    {
        for (int i = 0; i < n / 2 + 1; i++)
        {
            res(i) = v(2 * i);
        }
    }
    else if (ope == 2)
    {
        for (int i = 0; i < n / 2 - 1; i++)
        {
            res(i) = 0.25 * v(2 * i - 1) + 0.5 * v(2 * i) + 0.25 * v(2 * i + 1);
        }
    }
    return res;
}

Eigen::VectorXd DIM_NAMESPACE(One)::Prolongate(Operators::Interpolation ope, const Eigen::VectorXd &v)
{ // n/2+1
    int n = (v.size() - 1) * 2;
    VectorXd res = VectorXd::Zero(n + 1);
    switch (ope)
    {
    case Operators::Interpolation::Linear:
        for (int i = 0; i < n / 2 + 1; i++)
            res(2 * i) = v(i);
        for (int i = 0; i < n / 2; i++)
            res(2 * i + 1) = 0.5 * v(i) + 0.5 * v(i + 1);
        break;
    case Operators::Interpolation::Qudratic:
        res(0) = v(0);
        for (int i = 1; i < n / 2; i++)
        {
            res(2 * i) = v(i);
            res(2 * i - 1) = 0.375 * v(i - 1) + 0.75 * v(i) - 0.125 * v(i + 1);
        }
        res(n - 1) = 0.375 * v(n / 2) + 0.75 * v(n / 2 - 1) - 0.375 * v(n / 2 - 2);
        res(n) = v(n / 2);
        break;
    default:
        break;
    }
    return res;
}

Eigen::VectorXd DIM_NAMESPACE(Two)::Restriction(Operators::Restriction op, const Eigen::VectorXd &v) // n = 2^i
{
    int n = getN(v.size(), Two);
    SparseMatrix<double> m(pow(n / 2 + 1, 2), pow(n + 1, 2));
    int cnt = 0;
    if (op == Operators::Restriction::Injection)
    {
        for (int i = 0; i <= n / 2; i++)
        {
            for (int j = 0; j <= n / 2; j++)
            {
                m.insert(cnt, 2 * i * (n + 1) + 2 * j) = 1;
                cnt++;
            }
        }
        m.finalize();
        // cout << m << endl;
    }
    else if (op == Operators::Restriction::FullWeighting)
    {
        for (int i = 0; i <= n / 2; i++)
        {
            m.insert(cnt, 2 * i) = 1;
            cnt++;
        }
        for (int i = 1; i <= n / 2 - 1; i++)
        {
            m.insert(cnt, 2 * i * (n + 1)) = 1;
            cnt++;
            for (int j = 1; j <= n / 2 - 1; j++)
            {
                m.insert(cnt, 2 * i * (n + 1) + 2 * j) = 0.5;
                m.insert(cnt, 2 * i * (n + 1) + 2 * j + 1) = 0.0625;
                m.insert(cnt, 2 * i * (n + 1) + 2 * j - 1) = 0.0625;
                m.insert(cnt, 2 * (i - 1) * (n + 1) + 2 * j - 1) = 0.0625;
                m.insert(cnt, 2 * (i - 1) * (n + 1) + 2 * j) = 0.0625;
                m.insert(cnt, 2 * (i - 1) * (n + 1) + 2 * j + 1) = 0.0625;
                m.insert(cnt, 2 * (i + 1) * (n + 1) + 2 * j) = 0.0625;
                m.insert(cnt, 2 * (i + 1) * (n + 1) + 2 * j + 1) = 0.0625;
                m.insert(cnt, 2 * (i + 1) * (n + 1) + 2 * j - 1) = 0.0625;
                cnt++;
            }
            m.insert(cnt, 2 * i * (n + 1) + n) = 1;
            cnt++;
        }
        for (int i = 0; i <= n / 2; i++)
        {
            m.insert(cnt, n * (n + 1) + 2 * i) = 1;
            cnt++;
        }
        m.finalize();
    }
    return (m * v);
}

Eigen::VectorXd DIM_NAMESPACE(Two)::Prolongate(Operators::Interpolation op, const Eigen::VectorXd &v)
{
    int n = getN(v.size(), Two);
    SparseMatrix<double> M(pow(2 * n + 1, 2), pow(n + 1, 2));
    int cnt = 0;
    if (op == Operators::Interpolation::Linear)
    {
        for (int i = 0; i <= n - 1; i++)
        {
            for (int j = 0; j <= n - 1; j++) //(i, j) (2*i, 2*j)
            {
                M.insert(2 * i * (2 * n + 1) + 2 * j, i * (n + 1) + j) = 1;

                M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 0.5;
                M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 0.5;

                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, i * (n + 1) + j) = 0.5;
                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, (i + 1) * (n + 1) + j) = 0.5;

                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 0.25;
                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j) = 0.25;
                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 0.25;
                M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 1) = 0.25;
            }
            M.insert(2 * i * (2 * n + 1) + 2 * n, i * (n + 1) + n) = 1;
            M.insert((2 * i + 1) * (2 * n + 1) + 2 * n, i * (n + 1) + n) = 0.5;
            M.insert((2 * i + 1) * (2 * n + 1) + 2 * n, (i + 1) * (n + 1) + n) = 0.5;
            // cout << ++cnt << endl;
        }
        for (int j = 0; j <= n - 1; j++)
        {
            M.insert(2 * n * (2 * n + 1) + 2 * j, n * (n + 1) + j) = 1;
            M.insert(2 * n * (2 * n + 1) + 2 * j + 1, n * (n + 1) + j) = 0.5;
            M.insert(2 * n * (2 * n + 1) + 2 * j + 1, n * (n + 1) + j + 1) = 0.5;
        }
        M.insert(2 * n * (2 * n + 1) + 2 * n, n * (n + 1) + n) = 1;
        M.finalize();
    }
    else if (op == Operators::Interpolation::Qudratic)
    {
        for (int i = 0; i <= n; i++)
        {
            for (int j = 0; j <= n; j++) //(i, j) (2*i, 2*j)
            {
                /*************格点************/
                M.insert(2 * i * (2 * n + 1) + 2 * j, i * (n + 1) + j) = 1;

                if (j <= n - 2)
                {
                    /*************左边横粗网格线上*************/
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 0.375;
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 0.75;
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 2) = -0.125;
                }
                else if (j == n - 1)
                {
                    /*************最右侧横粗网格线上*************/
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 0.75;
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 0.375;
                    M.insert(2 * i * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j - 1) = -0.125;
                }

                /*************纵粗网格线上*************/
                if (i <= n - 2)
                {
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, i * (n + 1) + j) = 0.375;
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, (i + 1) * (n + 1) + j) = 0.75;
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, (i + 2) * (n + 1) + j) = -0.125;
                }
                else if (i == n - 1)
                {
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, i * (n + 1) + j) = 0.75;
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, (i + 1) * (n + 1) + j) = 0.375;
                    M.insert((2 * i + 1) * (2 * n + 1) + 2 * j, (i - 1) * (n + 1) + j) = -0.125;
                }

                /*************左边粗网格之间*************/
                if (i <= n - 2)
                {
                    if (j <= n - 2)
                    {
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 9.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 2) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 1) = 9.0 / 16;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 2) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j + 1) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j + 2) = 1.0 / 64;
                    }
                    else if (j == n - 1)
                    {
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 9.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j - 1) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j) = 9.0 / 16;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 1) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j - 1) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j + 1) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 2) * (n + 1) + j - 1) = 1.0 / 64;
                    }
                }
                else if (i == n - 1)
                {
                    if (j <= n - 2)
                    {
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 9.0 / 16;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 2) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j + 1) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j + 2) = 1.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j) = 9.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 1) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 2) = -3.0 / 64;
                    }
                    else if (j == n - 1)
                    {
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j) = 9.0 / 16;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j + 1) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, i * (n + 1) + j - 1) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j) = -3.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j + 1) = -3.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i - 1) * (n + 1) + j - 1) = 1.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j) = 9.0 / 32;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j + 1) = 9.0 / 64;
                        M.insert((2 * i + 1) * (2 * n + 1) + 2 * j + 1, (i + 1) * (n + 1) + j - 1) = -3.0 / 64;
                    }
                }
            }
            M.finalize();
        }
    }
    return (M * v);
}